'''
Created on Nov 25, 2010

@author: Polido
'''
from octorotor.Serial import Serial
import octorotor.joystick.Controler as Controler
from octorotor.SerialReader import SerialReader
from octorotor import DataPoint
from time import time

class Octorotor(object):
    '''
    classdocs
    '''

    def __init__(self, port, baud):
        self.Serial = Serial(port, baud)
        self.Joystick = Controler.Singleton
        self.serialReader = SerialReader(self)
        self.LastUpdate = 0
        
        self.dataPoints = []
        self.ValueRequest = None
        
        self.WriteCount = 0 # Number of requests made in a second
        self.LastWriteCount = 0
        self.NextRequestTime = 0
        
    def Remove(self):
        self.Serial.Remove()
        
    def writeJoystick(self):
        data = self.Joystick.getData()
        
        zVal = data.zVal
    
        if zVal < 0.0:
            zVal = 0.0
        
        self.Serial.start()
        self.Serial.writeNormalized( zVal, 0, 1. )
        self.Serial.writeNormalized( data.angle.p, -1., 1. )
        self.Serial.writeNormalized( data.angle.y, -1., 1. )
        self.Serial.writeNormalized( data.angle.r, -1., 1. )
        self.Serial.finish()
        
    def WriteSettings(self):
        if self.ValueRequest is None:
            return
        
        self.Serial.write('b')
        self.Serial.write( chr(self.ValueRequest.getRollPitchValue()) )
        self.Serial.write( chr(self.ValueRequest.getYawValue()) )
        self.Serial.write( chr(0xFF) )
        
    def SetValueRequest(self, requester):
        self.ValueRequest = requester
      
    def shouldStop(self):
        return self.Joystick.shouldStop()
      
    def write(self): 
        if self.shouldStop():
            self.Serial.write('e')
        else:
            self.Serial.write('a')
            self.writeJoystick() 
            
        self.WriteCount = self.WriteCount + 1
    
    def read(self):
        dataRead = self.Serial.readall()
        
        self.serialReader.appendData( dataRead )
        
        return len(dataRead)
    
    def writePerSeconds(self):
        return self.LastWriteCount
        
    def think(self):
        if self.read() > 0:
            self.write()
            
        if time() > self.NextRequestTime:
            self.LastWriteCount = self.WriteCount
            self.WriteCount = 0
            
            self.WriteSettings()
            self.NextRequestTime = time() + 1.0
        
    def processPacket(self, package):
        
        if package[0] == 1:
            data = DataPoint.DataPoint(package, self.Joystick.getData())
            #TODO: ATTETION: Memory leak - Never deleting items from the table
            if data.IsValid():
                if len(self.dataPoints) > 400:
                    del self.dataPoints[0]
                
                self.dataPoints.append(data)
                
        
        self.LastUpdate = time()
        
        
if time() % 1 == 0:
    print("Warning! System returning whole numbers for time()!")